#include "signalCodes.hpp"
#include <algorithm>
#include <cassert>

namespace hms
{

template<typename Derived>
ScalarTypeIfColumnVector<Derived> diff( const Eigen::DenseBase<Derived>& vec ){
	assert(vec.rows() == 2 && "Calling diff() only valid for 2D vectors!" );
	return vec[1] - vec[0];
}

/******************************************************************************/
/* exactlyEqual and overloads */
/******************************************************************************/
template<typename Derived, typename OtherDerived>
boolIfIntegral<typename Derived::Scalar, typename OtherDerived::Scalar>
exactlyEqual (
	const Eigen::DenseBase<Derived>& derived,
	const Eigen::DenseBase<OtherDerived>& other
){
	return derived.derived().matrix() == other.derived().matrix();
}

/******************************************************************************/
/* approxEqual and overloads */
/******************************************************************************/
template<typename Derived, typename OtherDerived>
bool approxEqual(
	const Eigen::DenseBase<Derived>& derived,
	const Eigen::DenseBase<OtherDerived>& other
){
	return derived.derived().matrix().isApprox(
		     other.derived().matrix(), hms::epsilon);
}


template<typename ... Ts>
boolIfScalars<Ts...> approxEqual( Ts ... vals ){
	std::array<scalar, sizeof...(Ts)> arr {{ vals ... }};
	for ( scalar s : arr ){
		if ( !approxEqual(arr[0], s) )
			return false;
	}
	return true;
}



template<typename T, typename... Ts>
void swapCols(
	Index a, Index b,
	Eigen::DenseBase<T>& firstToSwap,
	Eigen::DenseBase<Ts>& ... restToSwap
){
	#ifndef NDEBUG
	Index nCols { firstToSwap.cols() };
	assert( a >= 0 && a < nCols && b >= 0 && b < nCols );
	#endif

	firstToSwap.col(a).swap( firstToSwap.col(b) );

	if constexpr ( sizeof ... (restToSwap) > 0 ){
		swapCols(a, b, restToSwap ...);
	}
}


/******************************************************************************/
/* interpolate and overloads */
/******************************************************************************/

template<typename T>
auto interpolate( T a, T b, double ratio )
-> typename std::enable_if_t<std::is_floating_point_v<T>,T>
{
	return a + (b-a)*ratio;
}

template<typename Derived>
auto interpolate(
	const Eigen::DenseBase<Derived>& a,
	const Eigen::DenseBase<Derived>& b,
	double ratio
) -> decltype(auto) {
	return a.derived() + ( b.derived() - a.derived() )*ratio;
}


template<typename T>
bool sameEntries( const std::vector<T>& a, const std::vector<T>& b)
{
	if ( a.size() != b.size() ) return false;
	
	if ( a == b ) return true;
	
	
	for ( auto entry : a )
	{
		if ( std::find( b.cbegin(), b.cend(), entry ) == b.cend() )
			return false;
	}
	return true;
}


template<typename T>
auto hasEntry( const Ref<const ArrayXi>& v, T entry ) -> boolIfIntegral<T>
{
	return std::find( v.cbegin(), v.cend(), static_cast<Index>(entry) )
		!= v.cend();
}

template<typename T, typename U>
auto hasEntry( const std::vector<std::vector<T>>& vs, U entry ) ->
	boolIfIntegral<T,U>
{
	for ( const auto& v : vs )
	{
		if ( hasEntry ( v, entry ) )
		{
			return true;
		}
	}
	return false;
}



template<typename T, typename U>
auto hasEntry( const std::vector<T>& v, U entry ) -> boolIfIntegral<T,U>
{
	return std::find( v.cbegin(), v.cend(), static_cast<T>(entry) )
		!= v.cend();
}

template<typename T>
auto hasEntry( const std::vector<std::vector<T>>& va, const std::vector<T>& vb )
 -> boolIfIntegral<T>
{
	for ( const auto& v : va )
	{
		if (v == vb)
			return true;
	}
	return false;
}




/**
 * @brief copies the contents of an Eigen::Matrix to a std::vector
 * @param matr Eigen::Matrix with the contents to be copied
 * @return a std::vector with the same type as the input matrix, containing
 * a flattened copy of the matrix' contents.
 */
template<typename Derived>
std::vector<typename Derived::Scalar> toVector(
	const Eigen::DenseBase<Derived>& matr
){
	std::vector<typename Derived::Scalar> vec;
	vec.resize( matr.size() );
	ArrayX<typename Derived::Scalar>::Map( vec.data(), matr.size() ) =
		matr.reshaped();
	return vec;
}

template<typename Derived, typename T>
auto toEigen( const std::vector<T>& vec )
	-> std::enable_if_t<std::is_fundamental_v<T>, Derived>
{
	static_assert( std::is_base_of_v<Eigen::DenseBase<Derived>, Derived>,
		"Specified type must be an Eigen::DenseBase type!" );

	static_assert( std::is_convertible_v<T, typename Derived::Scalar>,
		"Incompatible types!" );

	static_assert(
		Derived::RowsAtCompileTime == 1 ||
		Derived::ColsAtCompileTime == 1,
		"Type must be row vector or column vector!" );

	Derived d;
	if constexpr (
		Derived::RowsAtCompileTime == Eigen::Dynamic ||
		Derived::ColsAtCompileTime == Eigen::Dynamic
	)
		d.resize( vec.size() );
	else
		assert( vec.size() <= static_cast<decltype(vec.size())>( d.size() ) && 
		"Fixed size Eigen vector too small!");
	
	/* copy existing values */
	auto dIter { d.begin() };
	for ( auto val : vec )
		*(dIter++) = val;
	/* fill up remaining values with signal code */
	for ( auto i{ vec.size() }; i < static_cast<decltype(i)>( d.size() ); ++i )
		d(i) = signalCode::uninitialised;
	
	return d;
}

template<typename Derived1, typename Derived2>
auto toEigen( const std::vector<Derived2>& vec )
	-> std::enable_if_t
		<std::is_base_of_v<Eigen::DenseBase<Derived2>, Derived2>, Derived1>
{
	static_assert( std::is_base_of_v<Eigen::DenseBase<Derived1>, Derived1>,
		"Template type must be an Eigen::DenseBase type!" );
	
	static_assert( std::is_convertible_v<
		typename Derived2::Scalar, typename Derived1::Scalar>,
		"Scalar types must be convertible!" );
	
	static_assert( Derived2::RowsAtCompileTime != Eigen::Dynamic &&
		Derived2::ColsAtCompileTime == 1,
		"std::vector template type must be fixed-size column vector!" );
	
	static_assert( Derived1::RowsAtCompileTime == Eigen::Dynamic ||
		static_cast<Index>(Derived1::RowsAtCompileTime) == 
		static_cast<Index>(Derived2::RowsAtCompileTime),
		"Specified return type must have dynamic row count or same row count as"
		" std::vector template type!" );

	static_assert( Derived1::ColsAtCompileTime == Eigen::Dynamic,
		"Return type must have dynamic column count, because size of "
		"std::vector is dynamic!" );
	
	Derived1 d ( Derived2::RowsAtCompileTime, vec.size() );
	for ( int i{0}; i<d.cols(); ++i ){
		d.col(i) = vec[i];
	}
	return d;
}


template<typename T>
auto findByName( const T& t, const std::string& name ) ->
	typename T::const_iterator
{
	return std::lower_bound(
		t.cbegin(), t.cend(), name
		, []( const auto& a, const auto& b ){ return a.name() < b; }
	);
}


template<typename Key, typename Value>
auto findKey(
	const std::vector<std::pair<Key, Value>>& v,
	const Key& key
) -> typename std::vector<std::pair<Key, Value>>::const_iterator
{
	static_assert( std::is_default_constructible_v<Value> );
	assert ( std::is_sorted(v.cbegin(), v.cend(),
	 	[](const auto& a, const auto& b){ return a.first < b.first; } ) &&
		"Container of key-value-pairs must be sorted!" );
	
	return std::lower_bound(
		v.cbegin(), v.cend(), 
		std::pair<Key, Value>{ key, {} },
		[](const auto& a, const auto& b){ return a.first < b.first; }
	);
}

template<typename Key, typename Value>
int getKeyIndex(
	const std::vector<std::pair<Key, Value>>& v,
	const Key& key
){
	auto iter { findKey(v,key) };
	if ( iter != v.cend() && iter->first == key )
		return iter - v.cbegin();
	else {
		std::stringstream ss; ss << "Key " << key << " not found!";
		throw( std::runtime_error( ss.str() ) );
	}
}

template<typename Key, typename Value>
void sortByKeys( std::vector<std::pair<Key, Value>>& v ){
	std::sort( v.begin(), v.end(),
		[](const auto& a, const auto& b){ return a.first < b.first; }
	);
}


template<typename IntegerType>
std::pair<IntegerType, IntegerType> ompSegment( IntegerType n ){
	static_assert( std::is_integral_v<IntegerType> );
	#ifdef _OPENMP
		IntegerType
			nLen { n / omp_get_num_threads() },
			nRem { n % omp_get_num_threads() };

		IntegerType nBeg { nLen * omp_get_thread_num() };
		nLen += (omp_get_thread_num() == omp_get_num_threads() - 1) ? nRem : 0;

		return { nBeg, nLen };
	#else
		return {0, n};
	#endif
}

template<typename Derived>
std::string sizeString( const Eigen::EigenBase<Derived>& obj ){
	std::stringstream s;
	s << obj.rows() << " x " << obj.cols();
	return s.str();
}

} // end namespace hms